//
// Created by 74354 on 2022/1/2.
//
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "stm32f4xx_hal.h"
#include "main.h"
#include "cmsis_os.h"
#include "motorTask.h"
#include "boardInit.h"
#include "commuTask.h"
#include "Motor.hpp"
#include "tim.h"
#include "task.h"
#include "myFliter.hpp"
#include "string.h"
#include "gdtr_define.h"

// float time = 0,T_str,T_end;
// float id,iq;
// int angle = 0;
// uint32_t count = 0;


extern osMessageQueueId_t msgLogInfo_id;
extern osMessageQueueId_t msgMotorInfo_id;
extern osSemaphoreId smpMotorReady_id;
msgMotorInfoStructure motor0Info;

void motorTaskFunc(void *argument)
{
    init_motor();
    init_drv8301();
    init_adc_driverTimer();
    init_foc();
    init_encoder();
    motor0.calibrate_motor();
    motor0.foc_controller->set_PI_control_para(1.5,0.15,1.5,0.15);
    //motor0.foc_controller->set_posControler_para(8,0.5,8);
    motor0.foc_controller->set_posControler_para(6,0.0005,0);
    myFilter::Kalman1P<float> kalFilter0(1,0.01,2,0.5);
    myFilter::Kalman1P<float> kalFilter1(1,0.01,2,0.5);
    motor0.foc_controller->kalman_I_D = &kalFilter0;
    motor0.foc_controller->kalman_I_Q = &kalFilter1;
    motor0.calibrate_encoder();
    motor0.foc_controller->release();
    osSemaphoreRelease(smpMotorReady_id);
    osSemaphoreRelease(smpMotorReady_id);
    uint32_t count = 0;
    for(;;)
    {
        motor0Info.I_current = 0;
        motor0Info.motor_id = 0;
        motor0Info.motorAngle = 0;
        
        //char* infoLog = (char*)malloc(50);
        //USER_LOG_SPRINTF(infoLog,USER_LOG_INFO,"count:%d",count);
        //sendLogMsg(msgLogInfo_id,&infoLog);
        USER_LOG(USER_LOG_INFO,"count:%d",count);
        osMessageQueuePut(msgMotorInfo_id,&motor0Info,0,0x40);
        //T_str = getTimeNow();

        //motor0.foc_controller->backward();
        // motor0.foc_controller->positionControl(100);
        // osDelay(2000);
        // motor0.foc_controller->positionControl(0);
        // osDelay(2000);
//        s[3] = motor0.encoder->getRadian();
        //T_end = getTimeNow();
        //s[0] = T_end - T_str;
//        s[0] = motor0.encoder->getAngle();
//        s[1] = motor0.foc_controller->i_d;
//        s[2] = motor0.foc_controller->i_q;
        //s[1] = motor0.encoder->getRadian();
        //s[2] = motor0.encoder->getAngle()*7;
        //s[3] = motor0.encoder->getRadian()*7;
        //sendWave(s,5);
///////////// 下面是电流环调试到理想的代码 ///////////////////
//        angle+=20;
//        motor0.foc_controller->u_alpha = cos(PI*angle/180)/3;
//        motor0.foc_controller->u_beta = sin(PI*angle/180)/2;
//        motor0.foc_controller->svpwm();

//        motor0.foc_controller->i_q = 0;
//        motor0.foc_controller->i_d = 0;
//        motor0.foc_controller->backward(motor0.iABC_ANO[0],motor0.iABC_ANO[1],motor0.iABC_ANO[2]);

//        count ++;
//        if(count % 4000 == 0)s[5] = 1 - s[5];
//        //s[2] = 1;
//        motor0.foc_controller->current_control(s[5],0);
//
//        s[3] = motor0.foc_controller->i_q;
//        s[4] = motor0.foc_controller->i_d;
        //s[5] = s[2];
        //s[2] = motor0.encoder->getRadian();


        //sendWave(s,6);

///////////// 上面是电流环的理想代码 ///////////////////


///////////// 下面是位置环代码      ///////////////////


//        s[0] = motor0.encoder->getRadian();
//        s[1] = motor0.encoder->getAngle();
//        s[2] = motor0.encoder->getRadian()/motor0.pole_pairs;
//        s[3] = motor0.encoder->getAngle()/motor0.pole_pairs;
//
//        sendWave(s,5);
//
//        float time1 = getTimeNow();
//        float e = time1 - time0;
//
//

//
//        int a[3];
//        float b[3];
//        b[0] = motor0.iABC_ANO[0];
//        b[1] = motor0.iABC_ANO[1];
//        b[2] = motor0.iABC_ANO[2];

//
//        a[0] = motor0.iABC_dig[0][0];
//        a[1] = motor0.iABC_dig[1][0];
//        a[2] = motor0.iABC_dig[2][0];
// 


//        //i_a = cos(ABS(Data_InjADC_M0[0])) + cos(ABS(Data_InjADC_M0[1]));
//        i_phase[0] = I_M0_DigToAno(Data_InjADC_M0[0]);
//        i_phase[1] = I_M0_DigToAno(Data_InjADC_M0[1]);

        //motor0.foc_controller->backward()
        //motor0.calibrate_motor();
        //motor0.calibrate_encoder();


        count++;
        osDelay(1);
    }
}






//
// Created by 74354 on 2022/1/2.
//

